#include <can_open_driver.h>

#include <rclcpp/rclcpp.hpp>

#include "motor_controller.h"

std::atomic<bool> CANopenDriver::m_runningState{true};

uint8_t writeCommand(size_t dataSize) {
  switch (dataSize) {
    case 1:
      return 0x2F;
    case 2:
      return 0x2B;
    case 3:
      return 0x27;
    case 4:
      return 0x23;
    default:
      return 0x00;
  }
}

uint8_t readCommand(size_t dataSize) {
  switch (dataSize) {
    case 1:
      return 0x4F;
    case 2:
      return 0x4B;
    case 3:
      return 0x47;
    case 4:
      return 0x43;
    default:
      return 0x02;
  }
}

CANopenDriver::CANopenDriver(std::string interface, int bitrate)
    : m_ifname(interface) {
  system(("sudo ip link set " + m_ifname + " down").c_str());
  std::string cmd = "sudo ip link set " + m_ifname + " type can bitrate " +
                    std::to_string(bitrate);
  if (system(cmd.c_str()) != 0) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to set bitrate");
    exit(EXIT_FAILURE);
  }
  system(("sudo ip link set " + m_ifname + " up").c_str());

  if ((m_sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Socket creation failed");
    exit(EXIT_FAILURE);
  }

  setsockopt(m_sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);

  strcpy(m_ifr.ifr_name, m_ifname.c_str());
  if (ioctl(m_sockfd, SIOCGIFINDEX, &m_ifr) < 0) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Interface index get failed");
    close(m_sockfd);
    exit(EXIT_FAILURE);
  }

  memset(&m_addr, 0, sizeof(m_addr));
  m_addr.can_family = AF_CAN;
  m_addr.can_ifindex = m_ifr.ifr_ifindex;
  if (bind(m_sockfd, (struct sockaddr*)&m_addr, sizeof(m_addr)) < 0) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Bind failed");
    close(m_sockfd);
    exit(EXIT_FAILURE);
  }

  struct can_filter filter;
  filter.can_id = 0x580;
  filter.can_mask = 0x7F0;
  if (setsockopt(m_sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, &filter,
                 sizeof(filter)) == -1) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "set CAN filter err");
  }
}

CANopenDriver::~CANopenDriver() { close(m_sockfd); }

bool CANopenDriver::sendFrame(const can_frame& frame) {
  if (write(m_sockfd, &frame, sizeof(frame)) != sizeof(frame)) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Write failed");
    m_errCounts++;
    if (m_errCounts > 100) {
      m_runningState = false;
    }
    return false;
  }
  m_errCounts = 0;
  return true;
}

bool CANopenDriver::receiveFrame(can_frame& frame, int timeout_ms) {
  fd_set readSet;
  FD_ZERO(&readSet);
  FD_SET(m_sockfd, &readSet);

  struct timeval timeout;
  timeout.tv_sec = timeout_ms / 1000;
  timeout.tv_usec = (timeout_ms % 1000) * 1000;
  int ret = select(m_sockfd + 1, &readSet, NULL, NULL, &timeout);
  if (ret > 0) {
    if (read(m_sockfd, &frame, sizeof(frame)) < 0) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Read failed");
      return false;
    }
    return true;
  }
  return false;
}

void CANopenDriver::sdoDownload(uint8_t node_id, uint16_t index,
                                uint8_t subindex, uint32_t data, uint8_t size) {
  can_frame frame;
  frame.can_id = 0x600 + node_id;  // 客户端 -> 服务器 SDO
  frame.can_dlc = 8;

  frame.data[0] = writeCommand(size);
  frame.data[1] = index & 0xFF;
  frame.data[2] = (index >> 8) & 0xFF;
  frame.data[3] = subindex;

  for (int i = 0; i < size; i++) {
    frame.data[4 + i] = (data >> (8 * i)) & 0xFF;
  }

  for (int i = size; i < 4; i++) {
    frame.data[4 + i] = 0;
  }
  can_frame response;
  {
    std::lock_guard<std::mutex> lock(m_mtx);
    if (!sendFrame(frame)) {
      return;
    }

    if (!receiveFrame(response)) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
                  "SDO download response timeout");
      return;
    }
  }

  if ((response.data[0] & 0xE0) != 0x60) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
                "Invalid SDO download response %u, write %u", response.data[0],
                (response.data[0] & 0xE0));
    return;
  }
}

void CANopenDriver::sdoUpload(uint8_t node_id, uint16_t index, uint8_t subindex,
                              uint32_t& data, uint8_t& size) {
  can_frame frame;
  frame.can_id = 0x600 + node_id;
  frame.can_dlc = 8;

  frame.data[0] = 0x40;
  frame.data[1] = index & 0xFF;
  frame.data[2] = (index >> 8) & 0xFF;
  frame.data[3] = subindex;
  memset(&frame.data[4], 0, 4);

  can_frame response;
  {
    std::lock_guard<std::mutex> lock(m_mtx);
    if (!sendFrame(frame)) {
      return;
    }

    if (!receiveFrame(response)) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SDO upload response timeout");
      return;
    }
  }

  if (readCommand(size) != response.data[0]) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
                "Invalid SDO upload response %u,read %u", response.data[0],
                size);
    return;
  }

  size = 4 - ((response.data[0] >> 2) & 0x03);
  data = 0;
  for (int i = 0; i < size; i++) {
    data |= (response.data[4 + i] << (8 * i));
  }
}

void CANopenDriver::sendPDO1(uint8_t pdo_num, const uint8_t* data,
                             uint8_t size) {
  can_frame frame;
  frame.can_id = 0x200 + pdo_num;
  frame.can_dlc = size;
  memcpy(frame.data, data, size);

  sendFrame(frame);
}

void CANopenDriver::sendPDO2(uint8_t pdo_num, const uint8_t* data,
                             uint8_t size) {
  can_frame frame;
  frame.can_id = 0x300 + pdo_num;
  frame.can_dlc = size;
  memcpy(frame.data, data, size);

  sendFrame(frame);
}

void CANopenDriver::receivePDO1(uint8_t pdo_num, uint8_t* data,
                                int timeout_ms) {
  can_frame frame;

  if (!receiveFrame(frame, timeout_ms)) {
    return;
  }

  if (frame.can_id != static_cast<uint32_t>(0x180 + pdo_num)) {
    return;
  }

  uint8_t size = frame.can_dlc;
  memcpy(data, frame.data, size);
}

void CANopenDriver::sendHeartBeat(uint8_t node_id) {
  can_frame frame;
  frame.can_id = 0x700 + node_id;
  frame.can_id |= CAN_RTR_FLAG;
  frame.can_dlc = 0;
  sendFrame(frame);
}

bool CANopenDriver::isCanDriverRunning() {
  return m_runningState;
}